#include "bucketcheck.h"
#include "location.h"
#include "camselect.h"
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/CameraInfo.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>


#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>


using namespace message_filters;
using namespace sensor_msgs;
using namespace Eigen;
using namespace std;
using namespace cv;
vector<long> data;
bool flag = false;

ros::Time stamp_;
std::basic_string<char> frame_id_;
ros::Time stamp_cam;
std::basic_string<char> frame_id_cam;
std::string map_frame_;

Mat img_left;
Mat img_disp(480,752,CV_16UC1);

void callback(const LaserScanConstPtr& laser, const ImageConstPtr& msg, const ImageConstPtr& msg2)  //回调中包含多个消息
{
  //jiguang
  if(flag == false){
    data.clear();
    for(int i=0;i<laser->ranges.size();i++)
    {
      if(laser->ranges[i] > 0.4 && laser->ranges[i] < 6.0){
        data.push_back(long(laser->ranges[i] * 1000));
      }else{
        data.push_back(0.0);
      }

      stamp_ = laser->header.stamp;
      frame_id_ = laser->header.frame_id;
    }
cout<<"data::"<<data.size()<<endl;
  //jiguangzuotu
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg, "rgb8");
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  img_left = cv_ptr->image;

  //jiguangshichatu
  cv_bridge::CvImagePtr cv_ptr1;
  try
  {
    cv_ptr1 = cv_bridge::toCvCopy(msg2, "16UC1");
  }
  catch (cv_bridge::Exception& e1)
  {
    ROS_ERROR("cv_bridge exception: %s", e1.what());
    return;
  }
    img_disp=cv_ptr1->image;
    stamp_cam=msg2->header.stamp;
  frame_id_cam=msg2->header.frame_id;
  flag = true;
  }
}


vector<double> cam_information;
void img_info(const sensor_msgs::CameraInfoPtr& msg3)
{
  cam_information.clear();
  for(int m=0;m<msg3->K.size();m++)
  cam_information.push_back(msg3->K[m]);
  msg3->header.frame_id=frame_id_cam;
  msg3->header.stamp=stamp_cam;
}

int
main(int argc, char** argv)
{
  vector<Point2f> location;
  Location location_bucket;

  ros::init(argc,argv,"bucketcheck");

  ros::NodeHandle nh_;
  ros::NodeHandle nh_private("~");
//  ros::Subscriber sub_ = nh_.subscribe("/scan",20,&scanCB);
  ros::Publisher marker_pub = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Publisher pub = nh_.advertise<geometry_msgs::PoseArray>("/bucket_pose",20);
  ros::Subscriber sub_info;
  nh_private.param("map_frame",map_frame_,std::string("map"));

  tf::TransformListener listener;

  typedef message_filters::sync_policies::ApproximateTime<LaserScan,Image,Image> laser_cam;
  message_filters::Subscriber<LaserScan>* laser_sub ;             // topic1 输入
  message_filters::Subscriber<Image>* left_sub;   // topic2 输入
  message_filters::Subscriber<Image>* disp_sub;
  message_filters::Synchronizer<laser_cam>* sync_;
  laser_sub = new message_filters::Subscriber<LaserScan>(nh_, "/scan", 1);
  left_sub = new message_filters::Subscriber<Image>(nh_, "/camera/rgb/image_raw", 1);
  disp_sub = new message_filters::Subscriber<Image>(nh_, "/camera/depth/image_raw", 1);

  sync_ = new  message_filters::Synchronizer<laser_cam>(laser_cam(10), *laser_sub, *left_sub,*disp_sub);
  sync_->registerCallback(boost::bind(&callback, _1, _2,_3));

  while(ros::ok()){
//visualization_msgs::Marker text;

//text.header.frame_id = "/map";
//text.header.stamp = ros::Time::now();
//text.ns = "basic_shapes";
//text.action = visualization_msgs::Marker::ADD;
//text.type = visualization_msgs::Marker::CYLINDER;
//text.pose.orientation.w =1.0;
//text.id = 0;
//text.scale.x=0.25;
//text.scale.y=0.25;
//text.scale.z=1.2;
//text.color.r=1.0f;
//text.color.a = 1.0;
//text.pose.position.x = 1;
//text.pose.position.y = 1;
//text.pose.position.z =0;
//text.lifetime = ros::Duration();
//marker_pub.publish(text);

   // cout<<"cannot run:2"<<endl;
    if(flag){
      clock_t start,finish;
      start = clock();
      vector<Eigen::VectorXf> model_circle;
      BucketCheck bucket;
      bucket.bucketcheck(data, model_circle);
      geometry_msgs::PoseArray pose_bucket;
      pose_bucket.header.stamp = stamp_;
      pose_bucket.header.frame_id = frame_id_;
      geometry_msgs::Pose bucket_pose;

      sub_info = nh_.subscribe("/camera/rgb/camera_info", 20, &img_info);

      if(img_left.cols!=0&&cam_information.size()!=0)
      {
        tf::StampedTransform transform;
        listener.lookupTransform(frame_id_,frame_id_cam,ros::Time(0),transform);
        float camera_height = transform.getOrigin().getZ() + 0.26;
        Camselect CamCheck;
        CamCheck.camcheck(camera_height,cam_information,img_left,img_disp,model_circle);


        tf::StampedTransform transform_cam;
        if(listener.waitForTransform(map_frame_,frame_id_,ros::Time(0),ros::Duration(3.0)))
        {
           listener.lookupTransform(map_frame_,frame_id_,ros::Time(0),transform_cam);
           Eigen::Isometry3d t1 = Eigen::Isometry3d::Identity();

           double* dt1 =  t1.data();

          dt1[0]=transform_cam.getBasis()[0][0];
          dt1[1]=transform_cam.getBasis()[1][0];
          dt1[2]=transform_cam.getBasis()[2][0];
          dt1[3]=0;
          dt1[4]=transform_cam.getBasis()[0][1];
          dt1[5]=transform_cam.getBasis()[1][1];
          dt1[6]=transform_cam.getBasis()[2][1];
          dt1[7]=0;
          dt1[8]=transform_cam.getBasis()[0][2];
          dt1[9]=transform_cam.getBasis()[1][2];
          dt1[10]=transform_cam.getBasis()[2][2];
          dt1[11]=0;
          dt1[12]=transform_cam.getOrigin().x();
          dt1[13]=transform_cam.getOrigin().y();
          dt1[14]=0;
          dt1[15]=1;

          vector<Vector4d> pose_laser;
          vector<Eigen::Vector4d> pose_array;
          cout<<"x::"<<transform_cam.getOrigin().x()<<"  y::"<<transform_cam.getOrigin().y()<<"   z::"<<transform_cam.getOrigin().z()<<endl;
          location_bucket.follow_location(pose_laser,model_circle,t1,location,pose_array);

          cout<<location.size()<<endl;
          for(int i=0;i<location.size();i++)
          {
             cout<<"第"<<i<<"个条桶坐标：  "<<location[i].x<<","<<location[i].y<<endl;
          }
          for(int i=0;i<pose_array.size();i++)
          {
             cout<<"当前视野中的条筒::"<<pose_array[i][0]<<","<<pose_array[i][1]<<endl;
          }
          Location::AreaGrid area_visual;
          vector<visualization_msgs::Marker> circles_rviz;
          cout<<"circles::"<<pose_array.size()<<endl;

//visualization_msgs::Marker text;
//text.header.frame_id = "/map";
//text.header.stamp = ros::Time::now();
//text.ns = "basic_shapes";
//text.action = visualization_msgs::Marker::ADD;
//text.type = visualization_msgs::Marker::CYLINDER;
//text.pose.orientation.w =1.0;
//text.id = 0;
//text.scale.x=0.25;
//text.scale.y=0.25;
//text.scale.z=1.2;
//text.color.r=1.0f;
//text.color.a = 1.0;
//text.pose.position.x = 1;
//text.pose.position.y = 1;
//text.pose.position.z =0;
//text.lifetime = ros::Duration();
//marker_pub.publish(text);
          if(pose_array.size()!=0)
            {
            location_bucket.areaVisualization(pose_array,area_visual,circles_rviz);
          }
          for(int i=0;i<circles_rviz.size();i++){
          marker_pub.publish(circles_rviz[i]);
          }


          for(int i=0;i<pose_array.size();i++)
          {
             bucket_pose.position.x=pose_array[i][0];
             bucket_pose.position.y=pose_array[i][1];
             bucket_pose.position.z=0;
             pose_bucket.poses.push_back(bucket_pose);
          }
          pub.publish(pose_bucket);
        }

//      imshow("img_left",img_left);
//      waitKey(1);

//      imshow("img_disp",img_disp);
//      waitKey(1);
      }
      flag = false;
    }

    ros::spinOnce();
  }
  return 0;
}
